

#include "DENSOrobot.hpp"


void DENSOrobot::getDefaultPose()
{
	JPose = Mat::zeros(1,6,CV_64F);

	JPose.at<double>(0) = 0.0/180.0*CV_PI;
	JPose.at<double>(1) = -45/180*CV_PI;
	JPose.at<double>(2) = 135/180*CV_PI;
	JPose.at<double>(3) = 0;
	JPose.at<double>(4) = 0;
	JPose.at<double>(5) = 0;
}

// This helps to call a different constructor other than default one - it is Constructor Initialization List
DENSOrobot::DENSOrobot(int use_RS232_flg): 
											serialPort(1)  // Joel's style different line
											
{

	DENSOrobot::use_RS232 = use_RS232_flg;

	//DENSOrobot();  // BUG caught -> the DH table initialized by this call is just local, not global DENSOrobot.DH

	DH.a1 = 0;			DH.a2 = 210;	DH.a3 = -75;		DH.a4 = 0;			DH.a5 = 0;			DH.a6 = 0;
	DH.alf1 = CV_PI/2;	DH.alf2 = 0;	DH.alf3 = CV_PI/2;	DH.alf4 = CV_PI/2;	DH.alf5 = CV_PI/2;	DH.alf6 = 0;
	DH.d1 = 0;			DH.d2 = 0;		DH.d3 = 0;			DH.d4 = 210;		DH.d5 = 0;			DH.d6 = 70;

	// init robot's pose by default
	getDefaultPose();   // this does not update the robot state!? why?!

	if (use_RS232_flg)
	{
		//serialPort = RS232(); // how to init it? -> automatically invoked already

		
		//printf("Port is opened! close it now!");
		//serialPort.Close();   // close the port opened by RS232 class's old open method		

		//serialPort = RS232(1);

		// This is how to access an enum
		serialPort.SetOperMode(RS232::OPER_MODE::OPER_READ_WRITE);
	}

}

/**
 * @brief Default initialization of InitPose is JPose
 * 
 * @param InitPose ...
 * @param use_RS232_flg ...
 */
DENSOrobot::DENSOrobot(const Mat InitPose, int use_RS232_flg)  // !!
{

	DENSOrobot::use_RS232 = use_RS232_flg;

	//DENSOrobot();  // BUG caught -> the DH table initialized by this call is just local, not global DENSOrobot.DH

	DH.a1 = 0;			DH.a2 = 210;	DH.a3 = -75;		DH.a4 = 0;			DH.a5 = 0;			DH.a6 = 0;
	DH.alf1 = CV_PI/2;	DH.alf2 = 0;	DH.alf3 = CV_PI/2;	DH.alf4 = CV_PI/2;	DH.alf5 = CV_PI/2;	DH.alf6 = 0;
	DH.d1 = 0;			DH.d2 = 0;		DH.d3 = 0;			DH.d4 = 210;		DH.d5 = 0;			DH.d6 = 70;

	// init robot's pose
	InitPose.copyTo(JPose);

	if (use_RS232_flg)
	{
		//serialPort = RS232(); // how to init it? -> automatically invoked already
	}
}

/*
DENSOrobot::DENSOrobot() //;(const Mat &dstPose, Mat &SOL)
{
	// Init DH parameters: 
	//Mat a = Mat(6,1, CV_64F, 0);
	//Mat alf = Mat(6,1, CV_64F, 0);
	//Mat d = Mat(6,1, CV_64F, 0);
	//double a[6] = {0,0,0,0,0,0};
	//double alf[6] = {0,0,0,0,0,0};
	//double d[6] = {0,0,0,0,0,0};

	//a[1] = 210; a[2] = -75; 
	//alf[0] = alf[2] = alf[3] = alf[4] = CV_PI/2;
	//d[3] = 210; d[5] = 70;

	DENSOrobot::use_RS232 = 0;

	//DENSOrobot::DH.a1 = 0;			DENSOrobot::DH.a2 = 210;	// this doesnot help to fix the code

	DH.a1 = 0;			DH.a2 = 210;	DH.a3 = -75;		DH.a4 = 0;			DH.a5 = 0;			DH.a6 = 0;
	DH.alf1 = CV_PI/2;	DH.alf2 = 0;	DH.alf3 = CV_PI/2;	DH.alf4 = CV_PI/2;	DH.alf5 = CV_PI/2;	DH.alf6 = 0;
	DH.d1 = 0;			DH.d2 = 0;		DH.d3 = 0;			DH.d4 = 210;		DH.d5 = 0;			DH.d6 = 70;

	// init robot's pose by default
	getDefaultPose();
	
}
*/


DENSOrobot::~DENSOrobot() 
{

	//serialPort.~RS232();

}

// Last Edited: 03/06/13
// Description: return Velocity Jacobian in m and rad
//				- input: JPose 6 joint angles (status var) in low level controller coord. sys.
//				- output: Vel Jacob matrix (Jacob), A_16 transformation matrix from ee to base coord. sys. which
//				          are all in internal coord. sys. (ref. to the original diagram (hand writing))

//void DENSOrobot::getVelJacob(Mat& Jacob)
/**
 * @brief (03/06/13) return Velocity Jacobian in m and rad
 * 
 * @param Jacob Vel Jacob matrix 
 * @param A_16 transformation matrix from ee to base coord. sys. which are all in internal coord. sys. (ref. to the original diagram (hand writing))
 * @return void
 */
void DENSOrobot::getVelJacob(Mat& Jacob, Mat& A_16)   // A_123456 (tranf mat from ee to base)
{
	// don't really need fi -> get from the robot's state JPose or Pose

	//CV_Assert(fi.rows == 1 && fi.cols == 6);
	//CV_Assert(fi.channels() == 1);
	//CV_Assert(fi.depth() == CV_64FC1);    // assert this to define access type

	Mat A1 = Mat::zeros(4, 4, CV_64F);
	Mat A2 = Mat::zeros(4, 4, CV_64F);
	Mat A3 = Mat::zeros(4, 4, CV_64F);
	Mat A4 = Mat::zeros(4, 4, CV_64F);
	Mat A5 = Mat::zeros(4, 4, CV_64F);
	Mat A6 = Mat::zeros(4, 4, CV_64F);

	// conversion of low lvl coord sys to int coord sys
	Mat JPoseTmp;
	JPose.copyTo(JPoseTmp);

	Mat dfii = Mat::zeros(1, 6, CV_64FC1);
	dfii.at<double>(0) = 0;
	dfii.at<double>(1) = -CV_PI/2;
	dfii.at<double>(2) = -CV_PI/2;
	dfii.at<double>(3) = -CV_PI;
	dfii.at<double>(4) = -CV_PI;
	dfii.at<double>(5) = -CV_PI;

	JPoseTmp = JPoseTmp - dfii;
	// end of conversion


	double fi1, fi2, fi3, fi4, fi5, fi6;

	// copy from the robot joint pose
	fi1 = JPoseTmp.at<double>(0);
	fi2 = JPoseTmp.at<double>(1);
	fi3 = JPoseTmp.at<double>(2);
	fi4 = JPoseTmp.at<double>(3);
	fi5 = JPoseTmp.at<double>(4);
	fi6 = JPoseTmp.at<double>(5);

	double cfi1 = cos(fi1);
	double cfi2 = cos(fi2);
	double cfi3 = cos(fi3);
	double cfi4 = cos(fi4);
	double cfi5 = cos(fi5);
	double cfi6 = cos(fi6);

	double sfi1 = sin(fi1);
	double sfi2 = sin(fi2);
	double sfi3 = sin(fi3);
	double sfi4 = sin(fi4);
	double sfi5 = sin(fi5);
	double sfi6 = sin(fi6);

	double calf1 = cos(DH.alf1);
	double calf2 = cos(DH.alf2);
	double calf3 = cos(DH.alf3);
	double calf4 = cos(DH.alf4);
	double calf5 = cos(DH.alf5);
	double calf6 = cos(DH.alf6);

	double salf1 = sin(DH.alf1);
	double salf2 = sin(DH.alf2);
	double salf3 = sin(DH.alf3);
	double salf4 = sin(DH.alf4);
	double salf5 = sin(DH.alf5);
	double salf6 = sin(DH.alf6);

	A1.at<double>(0,0) = cfi1;
	A1.at<double>(0,1) = -sfi1*calf1;
	A1.at<double>(0,2) = sfi1*salf1;
	A1.at<double>(0,3) = DH.a1*cfi1;   
	A1.at<double>(1,0) = sfi1;
	A1.at<double>(1,1) = cfi1*calf1;
	A1.at<double>(1,2) = -cfi1*salf1;
	A1.at<double>(1,3) = DH.a1*sfi1;
	A1.at<double>(2,0) = 0;
	A1.at<double>(2,1) = salf1;
	A1.at<double>(2,2) = calf1;
	A1.at<double>(2,3) = DH.d1;
	A1.at<double>(3,0) = 0;
	A1.at<double>(3,1) = 0;
	A1.at<double>(3,2) = 0;
	A1.at<double>(3,3) = 1;

	A2.at<double>(0,0) = cfi2;
	A2.at<double>(0,1) = -sfi2*calf2;
	A2.at<double>(0,2) = sfi2*salf2;
	A2.at<double>(0,3) = DH.a2*cfi2;
	A2.at<double>(1,0) = sfi2;
	A2.at<double>(1,1) = cfi2*calf2;
	A2.at<double>(1,2) = -cfi2*salf2;
	A2.at<double>(1,3) = DH.a2*sfi2;
	A2.at<double>(2,0) = 0;
	A2.at<double>(2,1) = salf2;
	A2.at<double>(2,2) = calf2;
	A2.at<double>(2,3) = DH.d2;
	A2.at<double>(3,0) = 0;
	A2.at<double>(3,1) = 0;
	A2.at<double>(3,2) = 0;
	A2.at<double>(3,3) = 1;

	A3.at<double>(0,0) = cfi3;
	A3.at<double>(0,1) = -sfi3*calf3;
	A3.at<double>(0,2) = sfi3*salf3;
	A3.at<double>(0,3) = DH.a3*cfi3;
	A3.at<double>(1,0) = sfi3;
	A3.at<double>(1,1) = cfi3*calf3;
	A3.at<double>(1,2) = -cfi3*salf3;
	A3.at<double>(1,3) = DH.a3*sfi3;
	A3.at<double>(2,0) = 0;
	A3.at<double>(2,1) = salf3;
	A3.at<double>(2,2) = calf3;
	A3.at<double>(2,3) = DH.d3;
	A3.at<double>(3,0) = 0;
	A3.at<double>(3,1) = 0;
	A3.at<double>(3,2) = 0;
	A3.at<double>(3,3) = 1;

	A4.at<double>(0,0) = cfi4;
	A4.at<double>(0,1) = -sfi4*calf4;
	A4.at<double>(0,2) = sfi4*salf4;
	A4.at<double>(0,3) = DH.a4*cfi4;
	A4.at<double>(1,0) = sfi4;
	A4.at<double>(1,1) = cfi4*calf4;
	A4.at<double>(1,2) = -cfi4*salf4;
	A4.at<double>(1,3) = DH.a4*sfi4;
	A4.at<double>(2,0) = 0;
	A4.at<double>(2,1) = salf4;
	A4.at<double>(2,2) = calf4;
	A4.at<double>(2,3) = DH.d4;
	A4.at<double>(3,0) = 0;
	A4.at<double>(3,1) = 0;
	A4.at<double>(3,2) = 0;
	A4.at<double>(3,3) = 1;


	A5.at<double>(0,0) = cfi5;
	A5.at<double>(0,1) = -sfi5*calf5;
	A5.at<double>(0,2) = sfi5*salf5;
	A5.at<double>(0,3) = DH.a5*cfi5;
	A5.at<double>(1,0) = sfi5;
	A5.at<double>(1,1) = cfi5*calf5;
	A5.at<double>(1,2) = -cfi5*salf5;
	A5.at<double>(1,3) = DH.a5*sfi5;
	A5.at<double>(2,0) = 0;
	A5.at<double>(2,1) = salf5;
	A5.at<double>(2,2) = calf5;
	A5.at<double>(2,3) = DH.d5;
	A5.at<double>(3,0) = 0;
	A5.at<double>(3,1) = 0;
	A5.at<double>(3,2) = 0;
	A5.at<double>(3,3) = 1;

	A6.at<double>(0,0) = cfi6;
	A6.at<double>(0,1) = -sfi6*calf6;
	A6.at<double>(0,2) = sfi6*salf6;
	A6.at<double>(0,3) = DH.a6*cfi6;
	A6.at<double>(1,0) = sfi6;
	A6.at<double>(1,1) = cfi6*calf6;
	A6.at<double>(1,2) = -cfi6*salf6;
	A6.at<double>(1,3) = DH.a6*sfi6;
	A6.at<double>(2,0) = 0;
	A6.at<double>(2,1) = salf6;
	A6.at<double>(2,2) = calf6;
	A6.at<double>(2,3) = DH.d6;
	A6.at<double>(3,0) = 0;
	A6.at<double>(3,1) = 0;
	A6.at<double>(3,2) = 0;
	A6.at<double>(3,3) = 1;

	// Unit conversion mm -> m
	A1(Range(0, 3), Range(3,4)) = A1(Range(0, 3), Range(3,4))/1000;   // RISK (Mat assignment) !!!! 
	A2(Range(0, 3), Range(3,4)) = A2(Range(0, 3), Range(3,4))/1000;
	A3(Range(0, 3), Range(3,4)) = A3(Range(0, 3), Range(3,4))/1000;
	A4(Range(0, 3), Range(3,4)) = A4(Range(0, 3), Range(3,4))/1000;
	A5(Range(0, 3), Range(3,4)) = A5(Range(0, 3), Range(3,4))/1000;
	A6(Range(0, 3), Range(3,4)) = A6(Range(0, 3), Range(3,4))/1000;

	// CALC JACOBIAN STARTS
	Mat e1 = Mat::zeros(3,1,CV_64F);
	Mat e2 = Mat::zeros(3,1,CV_64F);
	Mat e3 = Mat::zeros(3,1,CV_64F);
	Mat e4 = Mat::zeros(3,1,CV_64F);
	Mat e5 = Mat::zeros(3,1,CV_64F);
	Mat e6 = Mat::zeros(3,1,CV_64F);

	Mat B1 = Mat::zeros(3,1,CV_64F);
	Mat B2 = Mat::zeros(3,1,CV_64F);
	Mat B3 = Mat::zeros(3,1,CV_64F);
	Mat B4 = Mat::zeros(3,1,CV_64F);
	Mat B5 = Mat::zeros(3,1,CV_64F);
	Mat B6 = Mat::zeros(3,1,CV_64F);

	Mat tmp; 
	e1.at<double>(0) = 0; 
	e1.at<double>(1) = 0; 
	e1.at<double>(2) = 1; 
	B1.at<double>(0) = 0; 
	B1.at<double>(1) = 0; 
	B1.at<double>(2) = 0; 

	A1.copyTo(tmp);
	tmp(Range(0,3), Range(2,3)).copyTo(e2);
	tmp(Range(0,3), Range(3,4)).copyTo(B2);

	tmp = tmp*A2;
	tmp(Range(0,3), Range(2,3)).copyTo(e3);
	tmp(Range(0,3), Range(3,4)).copyTo(B3);

	tmp = tmp*A3;
	tmp(Range(0,3), Range(2,3)).copyTo(e4);
	tmp(Range(0,3), Range(3,4)).copyTo(B4);

	tmp = tmp*A4;
	tmp(Range(0,3), Range(2,3)).copyTo(e5);
	tmp(Range(0,3), Range(3,4)).copyTo(B5);

	tmp = tmp*A5;
	tmp(Range(0,3), Range(2,3)).copyTo(e6);
	tmp(Range(0,3), Range(3,4)).copyTo(B6);

	tmp = tmp*A6;
	tmp.copyTo(A_16);  // return forward transf matrix

	// debug code to check tmp;
	MATmtxdisp(tmp);

	Mat S1 = Mat::zeros(6,1,CV_64F);
	Mat S2 = Mat::zeros(6,1,CV_64F);
	Mat S3 = Mat::zeros(6,1,CV_64F);
	Mat S4 = Mat::zeros(6,1,CV_64F);
	Mat S5 = Mat::zeros(6,1,CV_64F);
	Mat S6 = Mat::zeros(6,1,CV_64F);

	e1.copyTo(S1(Range(0,3), Range(0,1)));
	e2.copyTo(S2(Range(0,3), Range(0,1)));
	e3.copyTo(S3(Range(0,3), Range(0,1)));
	e4.copyTo(S4(Range(0,3), Range(0,1)));
	e5.copyTo(S5(Range(0,3), Range(0,1)));
	e6.copyTo(S6(Range(0,3), Range(0,1)));

	(B1.cross(e1)).copyTo(tmp);				// RISK!! tmp is 4x4 mat or 3x1 mat?? (not yet reinit)
	// debug code to check tmp;
	MATmtxdisp(tmp);

	tmp.copyTo(S1(Range(3,6), Range(0,1)));
	(B2.cross(e2)).copyTo(tmp);
	tmp.copyTo(S2(Range(3,6), Range(0,1)));
	(B3.cross(e3)).copyTo(tmp);
	tmp.copyTo(S3(Range(3,6), Range(0,1)));
	(B4.cross(e4)).copyTo(tmp);
	tmp.copyTo(S4(Range(3,6), Range(0,1)));
	(B5.cross(e5)).copyTo(tmp);
	tmp.copyTo(S5(Range(3,6), Range(0,1)));
	(B6.cross(e6)).copyTo(tmp);
	tmp.copyTo(S6(Range(3,6), Range(0,1)));

	// RETURN JACOBIAN
	Jacob = Mat::zeros(6, 6, CV_64F);
	S1.copyTo(Jacob(Range(0,6), Range(0,1)));
	S2.copyTo(Jacob(Range(0,6), Range(1,2)));
	S3.copyTo(Jacob(Range(0,6), Range(2,3)));
	S4.copyTo(Jacob(Range(0,6), Range(3,4)));
	S5.copyTo(Jacob(Range(0,6), Range(4,5)));
	S6.copyTo(Jacob(Range(0,6), Range(5,6)));

	ofstream LoggerIn("VelJacob.log", ios_base::app);

	LoggerIn << "################ JPose ################" << endl;
	LoggerIn << JPose << endl;

	LoggerIn << "################ Vel Jacob ################" << endl;
	LoggerIn << Jacob << endl;
}


// Last edited: 03/06/13
// Description: - work with Radian angles
//				- input: fi 6 joint angles in low level controller coord. sys.
//				- output: transformation matrix from ee to base coord. sys. and A_i which
//				          are all in internal coord. sys. (ref. to the original diagram (hand writting))
/**
 * @brief Forward kinematic calculation
 * 
 * @param fi 6 joint angles in low level controller coord. sys.
 * @param A A_i which are all in internal coord. sys. (ref. to the original diagram (hand writing))
 * @return void
 */
void DENSOrobot::FKineSolve(const Mat fi, Mat& A)
{
	CV_Assert(fi.rows == 1 && fi.cols == 6);
	CV_Assert(fi.channels() == 1);
	CV_Assert(fi.depth() == CV_64FC1);    // assert this to define access type

	Mat A1 = Mat::zeros(4, 4, CV_64F);
	Mat A2 = Mat::zeros(4, 4, CV_64F);
	Mat A3 = Mat::zeros(4, 4, CV_64F);
	Mat A4 = Mat::zeros(4, 4, CV_64F);
	Mat A5 = Mat::zeros(4, 4, CV_64F);
	Mat A6 = Mat::zeros(4, 4, CV_64F);

	// conversion of low lvl coord sys to int coord sys
	Mat dfii = Mat::zeros(1, 6, CV_64FC1);
	dfii.at<double>(0) = 0;
	dfii.at<double>(1) = -CV_PI/2;
	dfii.at<double>(2) = -CV_PI/2;
	dfii.at<double>(3) = -CV_PI;
	dfii.at<double>(4) = -CV_PI;
	dfii.at<double>(5) = -CV_PI;

	Mat fiTmp;
	fiTmp = fi - dfii;
	// end of conversion

	double fi1, fi2, fi3, fi4, fi5, fi6;

	fi1 = fiTmp.at<double>(0,0);
	fi2 = fiTmp.at<double>(0,1);
	fi3 = fiTmp.at<double>(0,2);
	fi4 = fiTmp.at<double>(0,3);
	fi5 = fiTmp.at<double>(0,4);
	fi6 = fiTmp.at<double>(0,5);

	double cfi1 = cos(fi1);
	double cfi2 = cos(fi2);
	double cfi3 = cos(fi3);
	double cfi4 = cos(fi4);
	double cfi5 = cos(fi5);
	double cfi6 = cos(fi6);

	double sfi1 = sin(fi1);
	double sfi2 = sin(fi2);
	double sfi3 = sin(fi3);
	double sfi4 = sin(fi4);
	double sfi5 = sin(fi5);
	double sfi6 = sin(fi6);

	double calf1 = cos(DH.alf1);
	double calf2 = cos(DH.alf2);
	double calf3 = cos(DH.alf3);
	double calf4 = cos(DH.alf4);
	double calf5 = cos(DH.alf5);
	double calf6 = cos(DH.alf6);

	double salf1 = sin(DH.alf1);
	double salf2 = sin(DH.alf2);
	double salf3 = sin(DH.alf3);
	double salf4 = sin(DH.alf4);
	double salf5 = sin(DH.alf5);
   	double salf6 = sin(DH.alf6);

	A1.at<double>(0,0) = cfi1;
	A1.at<double>(0,1) = -sfi1*calf1;
	A1.at<double>(0,2) = sfi1*salf1;
	A1.at<double>(0,3) = DH.a1*cfi1;
	A1.at<double>(1,0) = sfi1;
	A1.at<double>(1,1) = cfi1*calf1;
	A1.at<double>(1,2) = -cfi1*salf1;
	A1.at<double>(1,3) = DH.a1*sfi1;
	A1.at<double>(2,0) = 0;
	A1.at<double>(2,1) = salf1;
	A1.at<double>(2,2) = calf1;
	A1.at<double>(2,3) = DH.d1;
	A1.at<double>(3,0) = 0;
	A1.at<double>(3,1) = 0;
	A1.at<double>(3,2) = 0;
	A1.at<double>(3,3) = 1;

	A2.at<double>(0,0) = cfi2;
	A2.at<double>(0,1) = -sfi2*calf2;
	A2.at<double>(0,2) = sfi2*salf2;
	A2.at<double>(0,3) = DH.a2*cfi2;
	A2.at<double>(1,0) = sfi2;
	A2.at<double>(1,1) = cfi2*calf2;
	A2.at<double>(1,2) = -cfi2*salf2;
	A2.at<double>(1,3) = DH.a2*sfi2;
	A2.at<double>(2,0) = 0;
	A2.at<double>(2,1) = salf2;
	A2.at<double>(2,2) = calf2;
	A2.at<double>(2,3) = DH.d2;
	A2.at<double>(3,0) = 0;
	A2.at<double>(3,1) = 0;
	A2.at<double>(3,2) = 0;
	A2.at<double>(3,3) = 1;

	A3.at<double>(0,0) = cfi3;
	A3.at<double>(0,1) = -sfi3*calf3;
	A3.at<double>(0,2) = sfi3*salf3;
	A3.at<double>(0,3) = DH.a3*cfi3;
	A3.at<double>(1,0) = sfi3;
	A3.at<double>(1,1) = cfi3*calf3;
	A3.at<double>(1,2) = -cfi3*salf3;
	A3.at<double>(1,3) = DH.a3*sfi3;
	A3.at<double>(2,0) = 0;
	A3.at<double>(2,1) = salf3;
	A3.at<double>(2,2) = calf3;
	A3.at<double>(2,3) = DH.d3;
	A3.at<double>(3,0) = 0;
	A3.at<double>(3,1) = 0;
	A3.at<double>(3,2) = 0;
	A3.at<double>(3,3) = 1;

	A4.at<double>(0,0) = cfi4;
	A4.at<double>(0,1) = -sfi4*calf4;
	A4.at<double>(0,2) = sfi4*salf4;
	A4.at<double>(0,3) = DH.a4*cfi4;
	A4.at<double>(1,0) = sfi4;
	A4.at<double>(1,1) = cfi4*calf4;
	A4.at<double>(1,2) = -cfi4*salf4;
	A4.at<double>(1,3) = DH.a4*sfi4;
	A4.at<double>(2,0) = 0;
	A4.at<double>(2,1) = salf4;
	A4.at<double>(2,2) = calf4;
	A4.at<double>(2,3) = DH.d4;
	A4.at<double>(3,0) = 0;
	A4.at<double>(3,1) = 0;
	A4.at<double>(3,2) = 0;
	A4.at<double>(3,3) = 1;


	A5.at<double>(0,0) = cfi5;
	A5.at<double>(0,1) = -sfi5*calf5;
	A5.at<double>(0,2) = sfi5*salf5;
	A5.at<double>(0,3) = DH.a5*cfi5;
	A5.at<double>(1,0) = sfi5;
	A5.at<double>(1,1) = cfi5*calf5;
	A5.at<double>(1,2) = -cfi5*salf5;
	A5.at<double>(1,3) = DH.a5*sfi5;
	A5.at<double>(2,0) = 0;
	A5.at<double>(2,1) = salf5;
	A5.at<double>(2,2) = calf5;
	A5.at<double>(2,3) = DH.d5;
	A5.at<double>(3,0) = 0;
	A5.at<double>(3,1) = 0;
	A5.at<double>(3,2) = 0;
	A5.at<double>(3,3) = 1;

	A6.at<double>(0,0) = cfi6;
	A6.at<double>(0,1) = -sfi6*calf6;
	A6.at<double>(0,2) = sfi6*salf6;
	A6.at<double>(0,3) = DH.a6*cfi6;
	A6.at<double>(1,0) = sfi6;
	A6.at<double>(1,1) = cfi6*calf6;
	A6.at<double>(1,2) = -cfi6*salf6;
	A6.at<double>(1,3) = DH.a6*sfi6;
	A6.at<double>(2,0) = 0;
	A6.at<double>(2,1) = salf6;
	A6.at<double>(2,2) = calf6;
	A6.at<double>(2,3) = DH.d6;
	A6.at<double>(3,0) = 0;
	A6.at<double>(3,1) = 0;
	A6.at<double>(3,2) = 0;
	A6.at<double>(3,3) = 1;

	A = A1*A2*A3*A4*A5*A6;
}

// Date created: 20/04/13
// Last Edited:  03/06/13
// Description: - work with Radian angles
//				- input: fi 6 joint angles in low level controller coord. sys.
//				- output: e1,...,e6; B1,...,B6
//				          transformation matrix from ee to base coord. sys. and A_i which
//				          are all in internal coord. sys. (ref. to the original diagram (hand writing))
/**
 * @brief Forward kinematic calculation
 * 
 * @param fi 6 joint angles in low level controller coord. sys.
 * @param e e1,...,e6
 * @param B B1,...,B6 e and B are additional returned arguments for further calculation
 * @param A A_i transformation matrix from ee to base coord. sys. which are all in internal coord. sys. (ref. to the original diagram (hand writing))
 * @return void
 */
void DENSOrobot::FKineSolve(const Mat fi, Mat& e, Mat& B, Mat& A)
{
	CV_Assert(fi.rows == 1 && fi.cols == 6);
	CV_Assert(fi.channels() == 1);
	CV_Assert(fi.depth() == CV_64FC1);    // assert this to define access type

	Mat A1 = Mat::zeros(4, 4, CV_64F);
	Mat A2 = Mat::zeros(4, 4, CV_64F);
	Mat A3 = Mat::zeros(4, 4, CV_64F);
	Mat A4 = Mat::zeros(4, 4, CV_64F);
	Mat A5 = Mat::zeros(4, 4, CV_64F);
	Mat A6 = Mat::zeros(4, 4, CV_64F);

	// conversion of low lvl coord sys to int coord sys
	Mat dfii = Mat::zeros(1, 6, CV_64FC1);
	dfii.at<double>(0) = 0;
	dfii.at<double>(1) = -CV_PI/2;
	dfii.at<double>(2) = -CV_PI/2;
	dfii.at<double>(3) = -CV_PI;
	dfii.at<double>(4) = -CV_PI;
	dfii.at<double>(5) = -CV_PI;

	Mat fiTmp = fi - dfii;
	// end of conversion

	double fi1, fi2, fi3, fi4, fi5, fi6;

	fi1 = fiTmp.at<double>(0,0);
	fi2 = fiTmp.at<double>(0,1);
	fi3 = fiTmp.at<double>(0,2);
	fi4 = fiTmp.at<double>(0,3);
	fi5 = fiTmp.at<double>(0,4);
	fi6 = fiTmp.at<double>(0,5);

	double cfi1 = cos(fi1);
	double cfi2 = cos(fi2);
	double cfi3 = cos(fi3);
	double cfi4 = cos(fi4);
	double cfi5 = cos(fi5);
	double cfi6 = cos(fi6);

	double sfi1 = sin(fi1);
	double sfi2 = sin(fi2);
	double sfi3 = sin(fi3);
	double sfi4 = sin(fi4);
	double sfi5 = sin(fi5);
	double sfi6 = sin(fi6);

	double calf1 = cos(DH.alf1);
	double calf2 = cos(DH.alf2);
	double calf3 = cos(DH.alf3);
	double calf4 = cos(DH.alf4);
	double calf5 = cos(DH.alf5);
	double calf6 = cos(DH.alf6);

	double salf1 = sin(DH.alf1);
	double salf2 = sin(DH.alf2);
	double salf3 = sin(DH.alf3);
	double salf4 = sin(DH.alf4);
	double salf5 = sin(DH.alf5);
	double salf6 = sin(DH.alf6);

	A1.at<double>(0,0) = cfi1;
	A1.at<double>(0,1) = -sfi1*calf1;
	A1.at<double>(0,2) = sfi1*salf1;
	A1.at<double>(0,3) = DH.a1*cfi1;
	A1.at<double>(1,0) = sfi1;
	A1.at<double>(1,1) = cfi1*calf1;
	A1.at<double>(1,2) = -cfi1*salf1;
	A1.at<double>(1,3) = DH.a1*sfi1;
	A1.at<double>(2,0) = 0;
	A1.at<double>(2,1) = salf1;
	A1.at<double>(2,2) = calf1;
	A1.at<double>(2,3) = DH.d1;
	A1.at<double>(3,0) = 0;
	A1.at<double>(3,1) = 0;
	A1.at<double>(3,2) = 0;
	A1.at<double>(3,3) = 1;

	A2.at<double>(0,0) = cfi2;
	A2.at<double>(0,1) = -sfi2*calf2;
	A2.at<double>(0,2) = sfi2*salf2;
	A2.at<double>(0,3) = DH.a2*cfi2;
	A2.at<double>(1,0) = sfi2;
	A2.at<double>(1,1) = cfi2*calf2;
	A2.at<double>(1,2) = -cfi2*salf2;
	A2.at<double>(1,3) = DH.a2*sfi2;
	A2.at<double>(2,0) = 0;
	A2.at<double>(2,1) = salf2;
	A2.at<double>(2,2) = calf2;
	A2.at<double>(2,3) = DH.d2;
	A2.at<double>(3,0) = 0;
	A2.at<double>(3,1) = 0;
	A2.at<double>(3,2) = 0;
	A2.at<double>(3,3) = 1;

	A3.at<double>(0,0) = cfi3;
	A3.at<double>(0,1) = -sfi3*calf3;
	A3.at<double>(0,2) = sfi3*salf3;
	A3.at<double>(0,3) = DH.a3*cfi3;
	A3.at<double>(1,0) = sfi3;
	A3.at<double>(1,1) = cfi3*calf3;
	A3.at<double>(1,2) = -cfi3*salf3;
	A3.at<double>(1,3) = DH.a3*sfi3;
	A3.at<double>(2,0) = 0;
	A3.at<double>(2,1) = salf3;
	A3.at<double>(2,2) = calf3;
	A3.at<double>(2,3) = DH.d3;
	A3.at<double>(3,0) = 0;
	A3.at<double>(3,1) = 0;
	A3.at<double>(3,2) = 0;
	A3.at<double>(3,3) = 1;

	A4.at<double>(0,0) = cfi4;
	A4.at<double>(0,1) = -sfi4*calf4;
	A4.at<double>(0,2) = sfi4*salf4;
	A4.at<double>(0,3) = DH.a4*cfi4;
	A4.at<double>(1,0) = sfi4;
	A4.at<double>(1,1) = cfi4*calf4;
	A4.at<double>(1,2) = -cfi4*salf4;
	A4.at<double>(1,3) = DH.a4*sfi4;
	A4.at<double>(2,0) = 0;
	A4.at<double>(2,1) = salf4;
	A4.at<double>(2,2) = calf4;
	A4.at<double>(2,3) = DH.d4;
	A4.at<double>(3,0) = 0;
	A4.at<double>(3,1) = 0;
	A4.at<double>(3,2) = 0;
	A4.at<double>(3,3) = 1;


	A5.at<double>(0,0) = cfi5;
	A5.at<double>(0,1) = -sfi5*calf5;
	A5.at<double>(0,2) = sfi5*salf5;
	A5.at<double>(0,3) = DH.a5*cfi5;
	A5.at<double>(1,0) = sfi5;
	A5.at<double>(1,1) = cfi5*calf5;
	A5.at<double>(1,2) = -cfi5*salf5;
	A5.at<double>(1,3) = DH.a5*sfi5;
	A5.at<double>(2,0) = 0;
	A5.at<double>(2,1) = salf5;
	A5.at<double>(2,2) = calf5;
	A5.at<double>(2,3) = DH.d5;
	A5.at<double>(3,0) = 0;
	A5.at<double>(3,1) = 0;
	A5.at<double>(3,2) = 0;
	A5.at<double>(3,3) = 1;

	A6.at<double>(0,0) = cfi6;
	A6.at<double>(0,1) = -sfi6*calf6;
	A6.at<double>(0,2) = sfi6*salf6;
	A6.at<double>(0,3) = DH.a6*cfi6;
	A6.at<double>(1,0) = sfi6;
	A6.at<double>(1,1) = cfi6*calf6;
	A6.at<double>(1,2) = -cfi6*salf6;
	A6.at<double>(1,3) = DH.a6*sfi6;
	A6.at<double>(2,0) = 0;
	A6.at<double>(2,1) = salf6;
	A6.at<double>(2,2) = calf6;
	A6.at<double>(2,3) = DH.d6;
	A6.at<double>(3,0) = 0;
	A6.at<double>(3,1) = 0;
	A6.at<double>(3,2) = 0;
	A6.at<double>(3,3) = 1;

	// output for velocity screw calculation
	e = Mat::zeros(3, 6, CV_64F);
	B = Mat::zeros(3, 6, CV_64F);

	Mat A_12     = A1*A2;
	Mat A_123    = A_12*A3;
	Mat A_1234   = A_123*A4;
	Mat A_12345  = A_1234*A5;
	Mat A_123456 = A_12345*A6;

	e.at<double>(0,0) = 0;
	e.at<double>(1,0) = 0;
	e.at<double>(2,0) = 1;
	B.at<double>(0,0) = 0;
	B.at<double>(1,0) = 0;
	B.at<double>(2,0) = 0;

	A1(Range(0,3), Range(2,3)).copyTo(e(Range(0,3), Range(1,2)));  // e2
	A_12(Range(0,3), Range(2,3)).copyTo(e(Range(0,3), Range(2,3)));  // e3
	A_123(Range(0,3), Range(2,3)).copyTo(e(Range(0,3), Range(3,4)));  // e4
	A_1234(Range(0,3), Range(2,3)).copyTo(e(Range(0,3), Range(4,5)));  // e5
	A_12345(Range(0,3), Range(2,3)).copyTo(e(Range(0,3), Range(5,6)));  // e6

	A1(Range(0,3), Range(3,4)).copyTo(B(Range(0,3), Range(1,2)));  // B2
	A_12(Range(0,3), Range(3,4)).copyTo(B(Range(0,3), Range(2,3)));  // B3
	A_123(Range(0,3), Range(3,4)).copyTo(B(Range(0,3), Range(3,4)));  // B4
	A_1234(Range(0,3), Range(3,4)).copyTo(B(Range(0,3), Range(4,5)));  // B5
	A_12345(Range(0,3), Range(3,4)).copyTo(B(Range(0,3), Range(5,6)));  // B6
	// end of e and B output

	A = A1*A2*A3*A4*A5*A6;
}

// 14/06/13 - upgraded to 2 types of input, i.e.,
//            + fnlpose = [x, y, z, r, p, y] (in user coord.) OR 
//			  + fnlpose = E = [nx ox ax x; ny oy ay y; nz oz az z; 0 0 0 1] (in int. coord.)
// Description: solve inverse kinematic problem for DENSO robot
//              has two types of input
/**
 * @brief Inverse kinematic module
 * 
 * @param fnlpose = [x, y, z, r, p, y] (in user coord.) OR fnlpose = E = [nx ox ax x; ny oy ay y; nz oz az z; 0 0 0 1] (in int. coord.)
 * @param SOL returned 8 solutions of inverse kinematic problem
 * @return void
 */
void DENSOrobot::IKineSolve(const Mat fnlpose, Mat& SOL)
{	
	//CV_Assert(fnlpose.rows == 1 && fnlpose.cols == 6);
	CV_Assert(fnlpose.channels() == 1);
	CV_Assert(fnlpose.depth() == CV_64FC1);    // assert this to define access type

	double nx, ny, nz, ox, oy, oz, ax, ay, az, X, Y, Z;
	Mat ExA6inv_col4;
	Mat E;
	if (fnlpose.rows == 1)
	{
		// fnlpose = [x, y, z, r, p, y]
		
		E = RPY_DENSO(fnlpose);

		MATmtxdisp(E);
		
		nx = E.at<double>(0,0);
		ny = E.at<double>(1,0);
		nz = E.at<double>(2,0);
		ox = E.at<double>(0,1);
		oy = E.at<double>(1,1);
		oz = E.at<double>(2,1);
		ax = E.at<double>(0,2);
		ay = E.at<double>(1,2);
		az = E.at<double>(2,2);

		//printf("ox=%0.2f,  oy=%0.2f,  oz=%0.2f\n",ox,oy,oz);

		// Compute X, Y, Z
		ExA6inv_col4 = E.col(3) - E.col(2)*DH.d6;
		X = ExA6inv_col4.at<double>(0);    
		Y = ExA6inv_col4.at<double>(1);
		Z = ExA6inv_col4.at<double>(2);

		//printf("X=%0.2f; Y=%0.2f; Z=%0.2f\n", X, Y, Z);
	}
	else
	{
		// fnlpose = E = [nx ox ax x; ny oy ay y; nz oz az z; 0 0 0 1]
		nx = fnlpose.at<double>(0,0);
		ny = fnlpose.at<double>(1,0);
		nz = fnlpose.at<double>(2,0);
		ox = fnlpose.at<double>(0,1);
		oy = fnlpose.at<double>(1,1);
		oz = fnlpose.at<double>(2,1);
		ax = fnlpose.at<double>(0,2);
		ay = fnlpose.at<double>(1,2);
		az = fnlpose.at<double>(2,2);

		// Compute X, Y, Z
		ExA6inv_col4 = fnlpose.col(3) - fnlpose.col(2)*DH.d6;
		X = ExA6inv_col4.at<double>(0);    
		Y = ExA6inv_col4.at<double>(1);
		Z = ExA6inv_col4.at<double>(2);
	}

	// Shift coordinates
	Mat dfii = Mat::zeros(1, 6, CV_64FC1);
	dfii.at<double>(0) = 0;
	dfii.at<double>(1) = -CV_PI/2;
	dfii.at<double>(2) = -CV_PI/2;
	dfii.at<double>(3) = -CV_PI;
	dfii.at<double>(4) = -CV_PI;
	dfii.at<double>(5) = -CV_PI;

	MATmtxdisp(dfii);


	double a1 = DH.a1;  double d1 = DH.d1;  double alf1 = DH.alf1;
	double a2 = DH.a2;  double d2 = DH.d2;  double alf2 = DH.alf2;
	double a3 = DH.a3;  double d3 = DH.d3;  double alf3 = DH.alf3;
	double a4 = DH.a4;  double d4 = DH.d4;  double alf4 = DH.alf4;
	double a5 = DH.a5;  double d5 = DH.d5;  double alf5 = DH.alf5;
	double a6 = DH.a6;  double d6 = DH.d6;  double alf6 = DH.alf6;

	if(( a4 == 0 )&&( a5 == 0 )&&( d5 == 0 ))
	{
		// 1st step -> fi3

		double r = (X*X + Y*Y + Z*Z - a2*a2 - a3*a3 - d4*d4)/(2*a2*sqrt(d4*d4+a3*a3));
		double sgm = d4/sqrt(d4*d4 + a3*a3);
		double cgm = a3/sqrt(d4*d4 + a3*a3);
		double gm = atan2(sgm, cgm);            

		double fi3p = gm + 2*atan(+sqrt((1 - r)/(1 + r)));     // dangerous!!! -> checked

		double fi3m = gm + 2*atan(-sqrt((1 - r)/(1 + r)));


		/*cout << "r = " << r << endl;
		cout << "gm = " << gm << endl;
		cout << "fi3p = " << fi3p << endl;
		cout << "fi3m = " << fi3m << endl;*/

		// 2nd step -> fi1
		r = 0; 
		sgm = -X;
		cgm = Y;
		gm = atan2(sgm, cgm);

		double fi1p = gm + 2*atan(+sqrt((1 - r)/(1 + r)));  // 1st fi1 plus

		double fi1m = gm + 2*atan(-sqrt((1 - r)/(1 + r))); // 1st fi1 minus

		
		// 3rd step -> fi2
		// pp case
		double A = -d4*cos(fi3p) + a3*sin(fi3p);
		double B = d4*sin(fi3p) + a2 + a3*cos(fi3p);
		double C = X*cos(fi1p) + Y*sin(fi1p);

		double fi2pp;
		if (( A != 0 ) && ( B != 0 ))
		{
			double s2pp = (B*Z - A*C)/(A*A + B*B);
			double c2pp = (B*C + A*Z)/(A*A + B*B);

			fi2pp = atan2(s2pp,c2pp);
		}
		else
		{
			printf("pp case: YOURE DEAD!\n");
		}
		

		// pm case
		A = -d4*cos(fi3m) + a3*sin(fi3m);
		B = d4*sin(fi3m) + a2 + a3*cos(fi3m);
		C = X*cos(fi1p) + Y*sin(fi1p);

		double fi2pm;
		if (( A != 0 ) && ( B != 0 ))
		{
			double s2pm = (B*Z - A*C)/(A*A + B*B);
			double c2pm = (B*C + A*Z)/(A*A + B*B);

			fi2pm = atan2(s2pm,c2pm);
		}
		else
		{
			printf("pm case: YOURE DEAD!\n");
		}


		// mp case
		A = -d4*cos(fi3p) + a3*sin(fi3p);
		B = d4*sin(fi3p) + a2 + a3*cos(fi3p);
		C = X*cos(fi1m) + Y*sin(fi1m);

		double fi2mp;
		if (( A != 0 ) && ( B != 0 ))
		{
			double s2mp = (B*Z - A*C)/(A*A + B*B);
			double c2mp = (B*C + A*Z)/(A*A + B*B);

			fi2mp = atan2(s2mp,c2mp);
		}
		else
		{
			printf("mp case: YOURE DEAD!\n");
		}


		// mm case
		A = -d4*cos(fi3m) + a3*sin(fi3m);
		B = d4*sin(fi3m) + a2 + a3*cos(fi3m);
		C = X*cos(fi1m) + Y*sin(fi1m);

		double fi2mm;
		if (( A != 0 ) && ( B != 0 ))
		{
			double s2mm = (B*Z - A*C)/(A*A + B*B);
			double c2mm = (B*C + A*Z)/(A*A + B*B);

			fi2mm = atan2(s2mm,c2mm);
		}
		else
		{
			printf("mm case: YOURE DEAD!\n");
		}

		/*cout << "fi2pp =  " << fi2pp << endl;
		cout << "fi2pm = " << fi2pm << endl;
		cout << "fi2mp = " << fi2mp << endl;
		cout << "fi2mm = " << fi2mm << endl;*/


		// 5th step -> fi5
		double ldpp = -(ax*cos(fi1p)*sin(fi2pp + fi3p) + ay*sin(fi1p)*sin(fi2pp + fi3p) - az*cos(fi2pp + fi3p));
		double ldpm = -(ax*cos(fi1p)*sin(fi2pm + fi3m) + ay*sin(fi1p)*sin(fi2pm + fi3m) - az*cos(fi2pm + fi3m));
		double ldmp = -(ax*cos(fi1m)*sin(fi2mp + fi3p) + ay*sin(fi1m)*sin(fi2mp + fi3p) - az*cos(fi2mp + fi3p));
		double ldmm = -(ax*cos(fi1m)*sin(fi2mm + fi3m) + ay*sin(fi1m)*sin(fi2mm + fi3m) - az*cos(fi2mm + fi3m));


		double fi5ppp = 2*atan(+sqrt((1 - ldpp)/(1 + ldpp))); // fi5ppp: fi5p got from ldpp
		double fi5ppm = 2*atan(-sqrt((1 - ldpp)/(1 + ldpp))); // fi5ppm: fi5m got from ldpp         
		double fi5pmp = 2*atan(+sqrt((1 - ldpm)/(1 + ldpm))); 
		double fi5pmm = 2*atan(-sqrt((1 - ldpm)/(1 + ldpm)));
		double fi5mpp = 2*atan(+sqrt((1 - ldmp)/(1 + ldmp)));
		double fi5mpm = 2*atan(-sqrt((1 - ldmp)/(1 + ldmp)));
		double fi5mmp = 2*atan(+sqrt((1 - ldmm)/(1 + ldmm)));
		double fi5mmm = 2*atan(-sqrt((1 - ldmm)/(1 + ldmm)));



		/*cout << "fi5ppp = " << fi5ppp << endl;
		cout << "fi5ppm = " << fi5ppm << endl;
		cout << "fi5pmp = " << fi5pmp << endl;
		cout << "fi5pmm = " << fi5pmm << endl;
		cout << "fi5mpp = " << fi5mpp << endl;
		cout << "fi5mpm = " << fi5mpm << endl;
		cout << "fi5mmp = " << fi5mmp << endl;
		cout << "fi5mmm = " << fi5mmm << endl;*/

		
		// 6th step -> fi4
		double cfi4ppp, sfi4ppp;
		double cfi4ppm, sfi4ppm;
		double cfi4pmp, sfi4pmp;
		double cfi4pmm, sfi4pmm;
		double cfi4mpp, sfi4mpp;
		double cfi4mpm, sfi4mpm;
		double cfi4mmp, sfi4mmp;
		double cfi4mmm, sfi4mmm;

		cs4Cal_v20(fi1p,fi2pp,fi3p,fi5ppp,ax,ay,az, cfi4ppp, sfi4ppp); // created from fi5ppp
		cs4Cal_v20(fi1p,fi2pp,fi3p,fi5ppm,ax,ay,az, cfi4ppm, sfi4ppm); // created from fi5ppm
		cs4Cal_v20(fi1p,fi2pm,fi3m,fi5pmp,ax,ay,az, cfi4pmp, sfi4pmp);
		cs4Cal_v20(fi1p,fi2pm,fi3m,fi5pmm,ax,ay,az, cfi4pmm, sfi4pmm);
		cs4Cal_v20(fi1m,fi2mp,fi3p,fi5mpp,ax,ay,az, cfi4mpp, sfi4mpp);   
		cs4Cal_v20(fi1m,fi2mp,fi3p,fi5mpm,ax,ay,az, cfi4mpm, sfi4mpm);
		cs4Cal_v20(fi1m,fi2mm,fi3m,fi5mmp,ax,ay,az, cfi4mmp, sfi4mmp);
		cs4Cal_v20(fi1m,fi2mm,fi3m,fi5mmm,ax,ay,az, cfi4mmm, sfi4mmm); 

/*
		cout << "cfi4ppp = " << cfi4ppp << endl;
		cout << "cfi4ppm = " << cfi4ppm << endl;
		cout << "cfi4pmp = " << cfi4pmp << endl;
		cout << "cfi4pmm = " << cfi4pmm << endl;
		cout << "cfi4mpp = " << cfi4mpp << endl;
		cout << "cfi4mpm = " << cfi4mpm << endl;
		cout << "cfi4mmp = " << cfi4mmp << endl;
		cout << "cfi4mmm = " << cfi4mmm << endl;

		cout << "sfi4ppp = " << sfi4ppp << endl;
		cout << "sfi4ppm = " << sfi4ppm << endl;
		cout << "sfi4pmp = " << sfi4pmp << endl;
		cout << "sfi4pmm = " << sfi4pmm << endl;
		cout << "sfi4mpp = " << sfi4mpp << endl;
		cout << "sfi4mpm = " << sfi4mpm << endl;
		cout << "sfi4mmp = " << sfi4mmp << endl;
		cout << "sfi4mmm = " << sfi4mmm << endl;
*/



		double fi4ppp = atan2(sfi4ppp,cfi4ppp);  
		double fi4ppm = atan2(sfi4ppm,cfi4ppm);
		double fi4pmp = atan2(sfi4pmp,cfi4pmp);
		double fi4pmm = atan2(sfi4pmm,cfi4pmm);
		double fi4mpp = atan2(sfi4mpp,cfi4mpp);
		double fi4mpm = atan2(sfi4mpm,cfi4mpm);
		double fi4mmp = atan2(sfi4mmp,cfi4mmp);
		double fi4mmm = atan2(sfi4mmm,cfi4mmm);

		
		
		/*cout << "fi4ppp = " << fi4ppp << endl;
		cout << "fi4ppm = " << fi4ppm << endl;
		cout << "fi4pmp = " << fi4pmp << endl;
		cout << "fi4pmm = "  << fi4pmm << endl;
		cout << "fi4mpp = " << fi4mpp << endl;
		cout << "fi4mpm = " << fi4mpm << endl;
		cout << "fi4mmp = " << fi4mmp << endl;
		cout << "fi4mmm = " << fi4mmm << endl;*/


		// 7th step -> fi6
		double App, Bpp;
		double Apm, Bpm;
		double Amp, Bmp;
		double Amm, Bmm;

		abCal_v20(fi1p,fi2pp,fi3p,nx,ny,nz,ox,oy,oz, App,Bpp);
		abCal_v20(fi1p,fi2pm,fi3m,nx,ny,nz,ox,oy,oz, Apm,Bpm);
		abCal_v20(fi1m,fi2mp,fi3p,nx,ny,nz,ox,oy,oz, Amp,Bmp);
		abCal_v20(fi1m,fi2mm,fi3m,nx,ny,nz,ox,oy,oz, Amm,Bmm);

		double cfi6ppp, sfi6ppp;
		double cfi6ppm, sfi6ppm;
		double cfi6pmp, sfi6pmp;
		double cfi6pmm, sfi6pmm;
		double cfi6mpp, sfi6mpp;
		double cfi6mpm, sfi6mpm;
		double cfi6mmp, sfi6mmp;
		double cfi6mmm, sfi6mmm;

		cs6Cal_v20(App,Bpp,fi5ppp, cfi6ppp, sfi6ppp);
		cs6Cal_v20(App,Bpp,fi5ppm, cfi6ppm, sfi6ppm);
		cs6Cal_v20(Apm,Bpm,fi5pmp, cfi6pmp, sfi6pmp);
		cs6Cal_v20(Apm,Bpm,fi5pmm, cfi6pmm, sfi6pmm);
		cs6Cal_v20(Amp,Bmp,fi5mpp, cfi6mpp, sfi6mpp);
		cs6Cal_v20(Amp,Bmp,fi5mpm, cfi6mpm, sfi6mpm);
		cs6Cal_v20(Amm,Bmm,fi5mmp, cfi6mmp, sfi6mmp);
		cs6Cal_v20(Amm,Bmm,fi5mmm, cfi6mmm, sfi6mmm);

		double fi6ppp = atan2(sfi6ppp,cfi6ppp);
		double fi6ppm = atan2(sfi6ppm,cfi6ppm);
		double fi6pmp = atan2(sfi6pmp,cfi6pmp);
		double fi6pmm = atan2(sfi6pmm,cfi6pmm);
		double fi6mpp = atan2(sfi6mpp,cfi6mpp);
		double fi6mpm = atan2(sfi6mpm,cfi6mpm);
		double fi6mmp = atan2(sfi6mmp,cfi6mmp);
		double fi6mmm = atan2(sfi6mmm,cfi6mmm);


		/*cout << "fi6ppp = " << fi6ppp << endl;
		cout << "fi6ppm = " << fi6ppm << endl;
		cout << "fi6pmp = " << fi6pmp << endl;
		cout << "fi6pmm = "  << fi6pmm << endl;
		cout << "fi6mpp = " << fi6mpp << endl;
		cout << "fi6mpm = " << fi6mpm << endl;
		cout << "fi6mmp = " << fi6mmp << endl;
		cout << "fi6mmm = " << fi6mmm << endl;*/


		// POST PROCESSING
		Mat SOLtmp = Mat::zeros(8, 6, CV_64FC1);

		SOLtmp.at<double>(0, 0) = fi1p;   // 1st
		SOLtmp.at<double>(0, 1) = fi2pp;
		SOLtmp.at<double>(0, 2) = fi3p;
		SOLtmp.at<double>(0, 3) = fi4ppp;
		SOLtmp.at<double>(0, 4) = fi5ppp;
		SOLtmp.at<double>(0, 5) = fi6ppp;

		SOLtmp.at<double>(1, 0) = fi1p;   // 2nd
		SOLtmp.at<double>(1, 1) = fi2pp;
		SOLtmp.at<double>(1, 2) = fi3p;
		SOLtmp.at<double>(1, 3) = fi4ppm;
		SOLtmp.at<double>(1, 4) = fi5ppm;
		SOLtmp.at<double>(1, 5) = fi6ppm;

		SOLtmp.at<double>(2, 0) = fi1p;   // 3rd
		SOLtmp.at<double>(2, 1) = fi2pm;
		SOLtmp.at<double>(2, 2) = fi3m;
		SOLtmp.at<double>(2, 3) = fi4pmp;
		SOLtmp.at<double>(2, 4) = fi5pmp;
		SOLtmp.at<double>(2, 5) = fi6pmp;

		SOLtmp.at<double>(3, 0) = fi1p;   // 4th
		SOLtmp.at<double>(3, 1) = fi2pm;
		SOLtmp.at<double>(3, 2) = fi3m;
		SOLtmp.at<double>(3, 3) = fi4pmm;
		SOLtmp.at<double>(3, 4) = fi5pmm;
		SOLtmp.at<double>(3, 5) = fi6pmm;

		SOLtmp.at<double>(4, 0) = fi1m;   // 5th
		SOLtmp.at<double>(4, 1) = fi2mp;
		SOLtmp.at<double>(4, 2) = fi3p;
		SOLtmp.at<double>(4, 3) = fi4mpp;
		SOLtmp.at<double>(4, 4) = fi5mpp;
		SOLtmp.at<double>(4, 5) = fi6mpp;

		SOLtmp.at<double>(5, 0) = fi1m;   // 6th
		SOLtmp.at<double>(5, 1) = fi2mp;
		SOLtmp.at<double>(5, 2) = fi3p;
		SOLtmp.at<double>(5, 3) = fi4mpm;
		SOLtmp.at<double>(5, 4) = fi5mpm;
		SOLtmp.at<double>(5, 5) = fi6mpm;

		SOLtmp.at<double>(6, 0) = fi1m;   // 7th
		SOLtmp.at<double>(6, 1) = fi2mm;
		SOLtmp.at<double>(6, 2) = fi3m;
		SOLtmp.at<double>(6, 3) = fi4mmp;
		SOLtmp.at<double>(6, 4) = fi5mmp;
		SOLtmp.at<double>(6, 5) = fi6mmp;

		SOLtmp.at<double>(7, 0) = fi1m;   // 8th
		SOLtmp.at<double>(7, 1) = fi2mm;
		SOLtmp.at<double>(7, 2) = fi3m;
		SOLtmp.at<double>(7, 3) = fi4mmm;
		SOLtmp.at<double>(7, 4) = fi5mmm;
		SOLtmp.at<double>(7, 5) = fi6mmm;


		//MATmtxdisp(SOLtmp);
		
		for (int i = 0; i < 8; i++)
		{
			SOLtmp.row(i) = SOLtmp.row(i) + dfii;//.row(0);  // dangerous!! -> checked
		}

		//MATmtxdisp(SOLtmp);

		SOLtmp.copyTo(SOL);		
	}
	else
	{
		printf("This condition is out of the scope (a4 = a5 = d5 = 0)");
	}
}


Mat DENSOrobot::RPY_DENSO(const Mat fnlpose)
{	
	// fnlpose = [x, y, z, r, p, y]
	CV_Assert(fnlpose.rows == 1 && fnlpose.cols == 6);
	CV_Assert(fnlpose.channels() == 1);
	CV_Assert(fnlpose.depth() == CV_64FC1);  

	double x = -fnlpose.at<double>(0);
	double y = -fnlpose.at<double>(1);
	double z = fnlpose.at<double>(2);
	double ROLL = fnlpose.at<double>(3);
	double PITCH = fnlpose.at<double>(4);
	double YAW = fnlpose.at<double>(5);

	Mat E = Mat::zeros(4,4,CV_64FC1);

	// No Use
	double nx = cos(ROLL)*cos(PITCH)*cos(YAW) - sin(ROLL)*sin(YAW);
	double ny = sin(ROLL)*cos(PITCH)*cos(YAW) + cos(ROLL)*sin(YAW);
	double nz = -sin(PITCH)*cos(YAW);
	double ox = -cos(ROLL)*cos(PITCH)*sin(YAW) - sin(ROLL)*cos(YAW);
	double oy = -sin(ROLL)*cos(PITCH)*sin(YAW) + cos(ROLL)*cos(YAW);
	double oz = sin(PITCH)*sin(YAW);
	double ax = cos(YAW)*sin(PITCH)*cos(ROLL) + sin(YAW)*sin(ROLL);
	double ay = sin(YAW)*sin(PITCH)*cos(ROLL) - cos(YAW)*sin(ROLL);
	double az = cos(PITCH)*cos(ROLL);
	// End of No Use

	//Mat tmp = rotz(CV_PI)*rotx(CV_PI)*rotx(ROLL)*roty(PITCH)*rotz(YAW)*roty(CV_PI/2);   // test to comment out (11/06/13)
	//Mat tmp = roty(CV_PI/2)*rotx(-CV_PI)*rotz(ROLL)*roty(-PITCH)*rotx(YAW); // test a doubt of RPY bug (11/06/13) -> checked -> this works WELL!!!
	Mat tmp = roty(CV_PI/2)*rotx(-CV_PI)*rotz(CV_PI)*rotz(ROLL)*roty(PITCH)*rotx(-YAW);
		
	//Mat tmp1 = E(Range(0,2), Range(0,2));   //.rowRange(0,2), E.colRange(0,2));

	//cout << "tmp = " << endl;
	//MATmtxdisp(tmp);

	//MATmtxdisp(rotz(CV_PI));
	//MATmtxdisp(rotx(CV_PI));
	//MATmtxdisp(rotx(ROLL));
	//printf("PITCH = %0.10f", PITCH);
	//MATmtxdisp(roty(PITCH));
	//MATmtxdisp(rotz(YAW));
	//MATmtxdisp(roty(CV_PI/2));
	

	tmp.copyTo(E(Range(0,3), Range(0,3)));    // sub range access with Mat

	//printf("E=\n");
	//MATmtxdisp(E(Range(0,3), Range(0,3)));


	E.at<double>(0, 3) = x;
	E.at<double>(1, 3) = y;
	E.at<double>(2, 3) = z;
	E.at<double>(3, 3) = 1;

	return E;
}


Mat DENSOrobot::rotx(double fi)
{

	Mat Rx = Mat::zeros(3, 3, CV_64FC1);

	double cfi = cos(fi);
	double sfi = sin(fi);

	Rx.at<double>(0,0) = 1;
	Rx.at<double>(1,1) = cfi;
	Rx.at<double>(1,2) = -sfi;
	Rx.at<double>(2,1) = sfi;
	Rx.at<double>(2,2) = cfi;

	return Rx;
}

Mat DENSOrobot::roty(double fi)
{

	Mat Ry = Mat::zeros(3, 3, CV_64FC1);

	double cfi = cos(fi);
	double sfi = sin(fi);

	Ry.at<double>(0,0) = cfi;
	Ry.at<double>(0,2) = sfi;
	Ry.at<double>(1,1) = 1;
	Ry.at<double>(2,0) = -sfi;
	Ry.at<double>(2,2) = cfi;

	return Ry;
}

Mat DENSOrobot::rotz(double fi)
{

	Mat Rz = Mat::zeros(3, 3, CV_64FC1);

	double cfi = cos(fi);
	double sfi = sin(fi);

	Rz.at<double>(0,0) = cfi;
	Rz.at<double>(0,1) = -sfi;
	Rz.at<double>(1,0) = sfi;
	Rz.at<double>(1,1) = cfi;
	Rz.at<double>(2,2) = 1;

	return Rz;
}


void DENSOrobot::cs4Cal_v20(double fi1, double fi2, double fi3, double fi5, double ax, double ay, double az, 
							double& cfi, double& sfi)
{
	/*cout << "fi1 = " << fi1 << endl;
	cout << "fi2 = " << fi2 << endl;
	cout << "fi3 = " << fi3 << endl;
	cout << "fi5 = " << fi5 << endl;
	cout << "ax = " << ax << endl;
	cout << "ay = " << ay << endl;
	cout << "az = " << az << endl;*/

	if ( sin(fi5) != 0 )
	{
		cfi = (cos(fi1)*cos(fi2 + fi3)*ax + sin(fi1)*cos(fi2 + fi3)*ay + sin(fi2 + fi3)*az)/sin(fi5);
		sfi = (sin(fi1)*ax - cos(fi1)*ay)/sin(fi5);       
	}
	else 
	{
		printf("cs4Cal_v20: YOURE DEAD!\n");
	}

}


void DENSOrobot::abCal_v20(double fi1, double fi2, double fi3, double nx, double ny, double nz, double ox, double oy, double oz, 
						   double& A, double& B)
{

	A = cos(fi1)*sin(fi2 + fi3)*ox + sin(fi1)*sin(fi2 + fi3)*oy - cos(fi2 + fi3)*oz;
	B = cos(fi1)*sin(fi2 + fi3)*nx + sin(fi1)*sin(fi2 + fi3)*ny - cos(fi2 + fi3)*nz;


	//printf("A=%0.2f, B=%0.2f, ", A, B);    // BUG!! BUG!! BUG!! -> NOT BUG!!
	//printf("test=%0.10f\n", cos(fi1)*sin(fi2+fi3)*ox + sin(fi1)*sin(fi2 + fi3)*oy);
	//printf("test=%0.10f\n", cos(fi1)*sin(fi2+fi3)*ox);
}

void DENSOrobot::cs6Cal_v20(double A, double B, double fi5, 
							double& cfi, double& sfi)
{

	if (( A != 0 ) || ( B != 0 ))
	{
		sfi = -A*sin(fi5)/(A*A + B*B);
		cfi = B*sin(fi5)/(A*A + B*B);
	}
	else
	{
		printf("A = %0.2f", A);
		printf("B = %0.2f\n", B);
		printf("cs6Cal_v20: YOURE DEAD!\n");
	}	
}


int DENSOrobot::checkOut(double* joints)
{
	// catch abs(qj[i]) > 180
	for (int i=0; i<6; i++)
		if (abs(joints[i]) > 180)  // = CV_PI
			if (joints[i] >= 0)   // reverse qj[i] sign
			{
				joints[i] = -(360 - abs(joints[i]));
			} else
			{
				joints[i] = (360 - abs(joints[i]));
			}

			return 1;
}

int DENSOrobot::checkOut(Mat& joints)
{
	// catch abs(qj[i]) > 180
	double tmp;
	for (int i=0; i<6; i++)
	{
		//tmp = joints.at<double>(2)/CV_PI*180;
		tmp = joints.at<double>(i);

		// added 13/05/13
		if (abs(tmp) > 2*CV_PI)
			tmp = fmod(tmp, 2*CV_PI);
		// end of adding 13/05/13

		if (abs(tmp) > CV_PI)  // = CV_PI
		{
			if (tmp >= 0)   // reverse qj[i] sign
			{
				tmp = -(2*CV_PI - abs(tmp));
			} else
			{
				tmp = (2*CV_PI - abs(tmp));
			}
		}
			
		joints.at<double>(i) = tmp;
	}

	return 1;
}






//void DENSOrobot::IKineSolve(const Mat fnlpose, Mat& SOL);
// OrgPose = [j1 j2 j3 j4 j5 j6] (rad) -> this avoids calculating twice Inv Kine
// DstPose = [x y z r p y] (rad & mm)
// v1.0 () (deprecated 14/10/14) deprecated since its new version with 'mode' is created
/**
 * @brief path planning/selection to destination pose (DstPose) and perform the move
 * 
 * @param OrgPose = [j1 j2 j3 j4 j5 j6] (rad) -> this avoids calculating twice Inv Kine
 * @param DstPose [x y z r p y] (rad & mm)
 * @param BestTraj returned best path selection
 * @return int
 */
int DENSOrobot::TrajPlanner(const Mat OrgPose, const Mat DstPose, Mat& BestTraj)
{
    int ret = 1;
	double* joints = new double[8];  // allocate/initialize a pointer

	// Solve all Traj for DestPose
	Mat SOL = Mat::zeros(8, 6, CV_64FC1);

	//void DENSOrobot::IKineSolve(const Mat fnlpose, Mat& SOL);
	IKineSolve(DstPose, SOL);
	
	//MATmtxdisp(SOL);

	// For Each Traj choose the best relative to OrgPose
	double Cost = INF;
	int id=0;
	double CostTmp;
	//Mat dPose = Mat::zeros(1, 6, CV_64FC1);

	Mat MatTmp = Mat::zeros(1, 6, CV_64FC1);

	for (int i = 0; i < 8; i++)
	{
		// checkOut first
		SOL.row(i).copyTo(MatTmp);  // OpenCV does not allow address reference to Mat.row
		//checkOut(SOL.row(i));
		checkOut(MatTmp);
		//MATmtxdisp(MatTmp);

		MatTmp.copyTo(SOL.row(i));
		//MATmtxdisp(SOL.row(i));

		//getchar();

		// Check the choice
		//MATmtxdisp(SOL.row(i));
		if (!checkSOL(SOL.row(i))) continue;

		CostTmp = norm(OrgPose, SOL.row(i), NORM_L2);  // careful!!

		if (CostTmp < Cost)
		{
			Cost = CostTmp;
			id = i;
		}
	}

	//id = 5;
	printf("Solution %dth\n",id);

	// if there's a good result, return it else not return
	if ( Cost == INF )
	{
		ret = 0;
	} else
	{
		// Return the best choice
		SOL.row(id).copyTo(BestTraj);
		//MATmtxdisp(BestTraj/CV_PI*180);

		// if RS232 is used, command is also transmitted
		if (DENSOrobot::use_RS232)
		{
			joints[0] = BestTraj.at<double>(0)/CV_PI*180;//0;   // Joint is in Degree
			Cost = joints[0];
			joints[1] = BestTraj.at<double>(1)/CV_PI*180;//-45;
			Cost = joints[1];
			joints[2] = BestTraj.at<double>(2)/CV_PI*180;//130;
			Cost = joints[2];
			joints[3] = BestTraj.at<double>(3)/CV_PI*180;//0;
			Cost = joints[3];
			joints[4] = BestTraj.at<double>(4)/CV_PI*180;//0;
			Cost = joints[4];
			joints[5] = BestTraj.at<double>(5)/CV_PI*180;//0;
			Cost = joints[5];
			joints[6] = 30;  // speed
			joints[7] = 1;   // gripper
			//joints[0] = 0;   // Joint is in Degree
			//joints[1] = -30;
			//joints[2] = 100;
			//joints[3] = 60;
			//joints[4] = 45;
			//joints[5] = 0;

			//joints[0] = -45;   // Joint is in Degree -> BUG
			//joints[1] = 58;
			//joints[2] = 132;
			//joints[3] = -360;
			//joints[4] = -11;
			//joints[5] = -45;
			//joints[6] = 30;  // speed
			//joints[7] = 1;   // gripper

			//joints[0] = -45;   // Joint is in Degree
			//joints[1] = 58;
			//joints[2] = 132;
			//joints[3] = 0;
			//joints[4] = -11;
			//joints[5] = -45;
			//joints[6] = 30;  // speed
			//joints[7] = 1;   // gripper

			//checkOut(joints);

			serialPort.Write(joints);

			// update current robot's state
			//BestTraj.copyTo(JPose);  // removed 14/05/13
		}

		BestTraj.copyTo(JPose);  // added 14/05/13
	}

	return ret;
}


// Joint Control: check workspace condition and send to RS232
// DstPose = [j1 j2 j3 j4 j5 j6] (rad)
/**
 * @brief Joint Control - check workspace condition and send to RS232
 * 
 * @param DstPose = [j1 j2 j3 j4 j5 j6] (rad)
 * @return int
 */
int DENSOrobot::JTrajPlanner(const Mat DstPose)
{
	int ret = 1;
	double* joints = new double[8];  // allocate/initialize a pointer

	Mat DstPoseTmp = Mat::zeros(1, 6, CV_64F);
	Mat MatTmp = Mat::zeros(1, 6, CV_64F);

	DstPose.copyTo(DstPoseTmp);

	// checkOut first
	DstPoseTmp.copyTo(MatTmp);  // OpenCV does not allow address reference to Mat.row
	checkOut(MatTmp);
	//MATmtxdisp(MatTmp);

	MatTmp.copyTo(DstPoseTmp);
	//MATmtxdisp(DstPoseTmp);

	//getchar();

	// Check the choice
	// comment out for debugging - disable workspace check - enabled again
	if (!checkSOL(DstPoseTmp)) 
	{
		printf("Pose Out of Workspace!!\n");
		ret = 0;
		return 0; // return error & escape !!!
	}

	// if RS232 is used, command is also transmitted
	double Cost;    // a temp var to debug "add watch"
	if (DENSOrobot::use_RS232)
	{
		joints[0] = DstPoseTmp.at<double>(0)/CV_PI*180;//0;   // Joint is in Degree
		Cost = joints[0];
		joints[1] = DstPoseTmp.at<double>(1)/CV_PI*180;//-45;
		Cost = joints[1];
		joints[2] = DstPoseTmp.at<double>(2)/CV_PI*180;//130;
		Cost = joints[2];
		joints[3] = DstPoseTmp.at<double>(3)/CV_PI*180;//0;
		Cost = joints[3];
		joints[4] = DstPoseTmp.at<double>(4)/CV_PI*180;//0;
		Cost = joints[4];
		joints[5] = DstPoseTmp.at<double>(5)/CV_PI*180;//0;
		Cost = joints[5];
		joints[6] = 30;  // speed
		joints[7] = 1;   // gripper
		//joints[0] = 0;   // Joint is in Degree
		//joints[1] = -30;
		//joints[2] = 100;
		//joints[3] = 60;
		//joints[4] = 45;
		//joints[5] = 0;

		//joints[0] = -45;   // Joint is in Degree -> BUG
		//joints[1] = 58;
		//joints[2] = 132;
		//joints[3] = -360;
		//joints[4] = -11;
		//joints[5] = -45;
		//joints[6] = 30;  // speed
		//joints[7] = 1;   // gripper

		//joints[0] = -45;   // Joint is in Degree
		//joints[1] = 58;
		//joints[2] = 132;
		//joints[3] = 0;
		//joints[4] = -11;
		//joints[5] = -45;
		//joints[6] = 30;  // speed
		//joints[7] = 1;   // gripper

		//checkOut(joints);

		serialPort.Write(joints);

		// update current robot's state
		//Mat(1,6, CV_64F, joints).copyTo(JPose);  // !! risk !!  // removed 13/05/13
		//DstPoseTmp.copyTo(JPose);                                 // added 13/05/13 - removed 14/05/13
	}

	DstPoseTmp.copyTo(JPose);                                 // added 14/05/13

	return ret;
}


/**
 * @brief check a joint pose whether it is within workrange
 * 
 * @param joints joint pose input
 * @return char
 */
char DENSOrobot::checkSOL(Mat& joints)
{
	char tmp = 1;
	//double q1j, q2j, q3j, q4j, q5j, q6j;
	double qj[6] = {0,0,0,0,0,0};

	/*qj[0] = joints.at<double>(0, 0);   // RISK !!! (if joints is a 1-D row vector)
	qj[1] = joints.at<double>(0, 1);
	qj[2] = joints.at<double>(0, 2);
	qj[3] = joints.at<double>(0, 3);
	qj[4] = joints.at<double>(0, 4);
	qj[5] = joints.at<double>(0, 5);*/

	qj[0] = joints.at<double>(0);
	qj[1] = joints.at<double>(1);
	qj[2] = joints.at<double>(2);
	qj[3] = joints.at<double>(3);
	qj[4] = joints.at<double>(4);
	qj[5] = joints.at<double>(5);
	
	//double a = 160/180*CV_PI; // WORST BUG EVER -> 'a' always has ZERO value
 
	if (abs(qj[0]) > 2.6878070480712675484)   // = 154/180*PI -> these calculations should be replaced by constants
		tmp = 0;
	else
	{
		//if ((qj[1] > 2.09439510239319549230) || (qj[1] < -1.44862327915529354884))  // = 120/180*CV_PI and -83/180*CV_PI
		if ((qj[1] > 1.7453292519943295769236907684886) || (qj[1] < -1.44862327915529354884))  // = 100/180*CV_PI and -83/180*CV_PI
			tmp = 0;
		else
		{
			if ((qj[2] > 2.617993877991494365385) || (qj[2] < 0.33161255787892261961))   // = 150/180*PI and 19/180*PI
				tmp = 0;
			else
			{
				if(abs(qj[3]) > 2.7925268031909273230779052295818)  // 160 deg
				{
					tmp = 0;
				}
				else
				{
					if (abs(qj[4]) > 1.72787595947438628115)    // 99/180*PI
						tmp = 0;
					else
					{

					}

				}
				
			}
		}
	}

	return tmp;
}


// Date Created: 08/05/13
// Description: update joint state value directly from caller
/**
 * @brief update joint state value directly from caller, deprecated by 14/10/14 and replaced by inputPose
 * 
 * @param JPose_inp input Pose
 * @return void
 */
void DENSOrobot::updateJPose(const Mat JPose_inp)
{
		JPose.at<double>(0) = JPose_inp.at<double>(0);
		JPose.at<double>(1) = JPose_inp.at<double>(1);
		JPose.at<double>(2) = JPose_inp.at<double>(2);
		JPose.at<double>(3) = JPose_inp.at<double>(3);
		JPose.at<double>(4) = JPose_inp.at<double>(4);
		JPose.at<double>(5) = JPose_inp.at<double>(5);
}


// v2.0 (14/10/14) update state value directly from caller depending on control mode (joint/xyz)
// int DENSOrobot::inputPose(const Mat Pose_inp, int mode = CTRL_MODE_JOINT)
// {
// 	if (mode == CTRL_MODE_JOINT)
// 	{
// 	    updateJPose(Pose_inp);
// 	    
// 	} else 
// 		if (mode == CTRL_MODE_XYZ)
// 		{
// 		      Pose_inp.copyTo(CPose);
// 		  
// 		} else
// 		{
// 		      // error occurs
// 		      return 0;
// 		}
// 		
// 	return 1;
// }



//void DENSOrobot::calcJointRate()
//{
//	// Create a probe to test output control!!!!
//	// get vel Jacobian -- stuck here!!
//	// Mat Jacob;
//	// DSrb.getVelJacob(Jacob);
//
////	Mat theta_d = VJacob.inv(DECOMP_SVD)*vel_screw;
//	
//	// check Singularity (added 21/05/13)
//	double det = determinant(VJacob);
//	cout << "Singularity check - Det value = " << det << endl << endl;
//	// end of checking
//
//	// unit conversion of vel_screw from m to mm
//	/*vel_screw.at<double>(0) = vel_screw.at<double>(0)*1000;
//	vel_screw.at<double>(1) = vel_screw.at<double>(1)*1000;
//	vel_screw.at<double>(2) = vel_screw.at<double>(2)*1000;*/
//
//	// processing
//	//if (abs(det) < 0.01)
//	//{
//	//	// add a floor to VJacob
//	//	double fl = 0.01;
//	//	Mat VJacob_mod = Mat::eye(VJacob.size(), VJacob.type())*fl;
//	//	//(VJacob_mod+VJacob).copyTo(VJacob_mod);
//	//	VJacob_mod = VJacob_mod+VJacob;
//	//	
//	//	//theta_d = VJacob_mod.inv(DECOMP_SVD)*vel_screw;
//	//	theta_d = VJacob_mod.inv(DECOMP_LU)*vel_screw;
//
//	//	// fixed singularity
//	//	det = determinant(VJacob_mod);
//	//	cout << "fixed Det value = " << det << endl;
//	//} else 
//	{
//		theta_d = VJacob.inv(DECOMP_SVD)*vel_screw;
//		//theta_d = VJacob.inv(DECOMP_LU)*vel_screw;
//	}
//
//	// check inversion
//	/*cout << "SVD" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_SVD));
//	cout << "LU" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_LU));
//	cout << "CHOLESKY" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_CHOLESKY));*/
//	// end check
//
//	ofstream LoggerIn("theta_d.log", ios_base::app);
//
//	LoggerIn << "################ theta_d ################" << endl;
//	LoggerIn << theta_d << endl;
//}


// v2.0 (14/10/14) add mode to distinct joint and xyz control modes
//void DENSOrobot::calcPose1(const Mat theta_d, const Mat JPose0, Mat& JPose1, int mode = CTRL_MODE_JOINT)
//{
//    if (mode == CTRL_MODE_JOINT)
//	{
//	    calcJPose1(theta_d, JPose0, JPose1);
//	    
//	} else 
//	  if (mode == CTRL_MODE_XYZ)
//	  {
//		// Cartesian XYZ mode
//		Mat screw = theta_d;
//		Mat CPose0 = JPose0;
//		
//		float rbdelta_t = 0.01;   // make it small to refine discretization. 0.
//
//		Mat CPose1 = CPose0 + screw*rbdelta_t;
//		
//		// check CPose1 within workspace (Optional)
//		
//		// return Cartesian Pose
//		CPose1.copyTo(JPose1);
//	    
//	  } else
//	  {
//	      return 0;
//	  }  
//	  
//	  return 1;   
//}


void DENSOrobot::calcJPose1(const Mat theta_d, const Mat JPose0, Mat& JPose1)
{
	double rbdelta_t = 0.01;   // make it small to refine discretization. 0.

	cout << "JPose0:" << endl;
	MATmtxdisp(JPose0);
	cout << "theta_d:" << endl;
	MATmtxdisp(theta_d);
	printf("delta_t = %0.2f\n", rbdelta_t);

	JPose1 = JPose0 + theta_d*rbdelta_t;   // RISK!! "=" is OK?! -> OK

	// Tuning to be in workspace
	double rbdelta_t_tuned = rbdelta_t;

	Mat tmp;
	JPose1.copyTo(tmp);
	cout <<  "JPOse1 is:" << endl;
	MATmtxdisp(tmp);

	//DENSOrobot DSrb;
	//DSrb.checkOut(tmp);
	checkOut(tmp);

	//while (!DSrb.checkSOL(tmp))
	while (!checkSOL(tmp))
	{
		rbdelta_t_tuned = rbdelta_t_tuned/2;
		JPose1 = JPose0 + theta_d*rbdelta_t_tuned;
		JPose1.copyTo(tmp);
		//DSrb.checkOut(tmp);
		checkOut(tmp);
	}
}

//void DENSOrobot::calcJointRate()
//{
//	// Create a probe to test output control!!!!
//	// get vel Jacobian -- stuck here!!
//	// Mat Jacob;
//	// DSrb.getVelJacob(Jacob);
//
////	Mat theta_d = VJacob.inv(DECOMP_SVD)*vel_screw;
//	
//	// check Singularity (added 21/05/13)
//	double det = determinant(VJacob);
//	cout << "Singularity check - Det value = " << det << endl << endl;
//	// end of checking
//
//	// unit conversion of vel_screw from m to mm
//	/*vel_screw.at<double>(0) = vel_screw.at<double>(0)*1000;
//	vel_screw.at<double>(1) = vel_screw.at<double>(1)*1000;
//	vel_screw.at<double>(2) = vel_screw.at<double>(2)*1000;*/
//
//	// processing
//	//if (abs(det) < 0.01)
//	//{
//	//	// add a floor to VJacob
//	//	double fl = 0.01;
//	//	Mat VJacob_mod = Mat::eye(VJacob.size(), VJacob.type())*fl;
//	//	//(VJacob_mod+VJacob).copyTo(VJacob_mod);
//	//	VJacob_mod = VJacob_mod+VJacob;
//	//	
//	//	//theta_d = VJacob_mod.inv(DECOMP_SVD)*vel_screw;
//	//	theta_d = VJacob_mod.inv(DECOMP_LU)*vel_screw;
//
//	//	// fixed singularity
//	//	det = determinant(VJacob_mod);
//	//	cout << "fixed Det value = " << det << endl;
//	//} else 
//	{
//		theta_d = VJacob.inv(DECOMP_SVD)*vel_screw;
//		//theta_d = VJacob.inv(DECOMP_LU)*vel_screw;
//	}
//
//	// check inversion
//	/*cout << "SVD" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_SVD));
//	cout << "LU" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_LU));
//	cout << "CHOLESKY" << endl;
//	MATmtxdisp(VJacob.inv(DECOMP_CHOLESKY));*/
//	// end check
//
//	ofstream LoggerIn("theta_d.log", ios_base::app);
//
//	LoggerIn << "################ theta_d ################" << endl;
//	LoggerIn << theta_d << endl;
//}



// v2.0 (WORKING 14/10/14) new version with mode variable
//int DENSOrobot::VelCtrl(const Mat VelScrew, int mode = CTRL_MODE_JOINT)
//{
//    
//    if (mode == CTRL_MODE_JOINT)
//    {
//	VelCtrl(VelScrew);
//	
//    } else 
//      if (mode == CTRL_MODE_XYZ)
//      {
//	  Mat CPose1, BestTraj;
//
//	  calcPose1(VelScrew, CPose, CPose1);
//	  
//	  TrajPlanner(CPose, CPose1, BestTraj, CTRL_MODE_XYZ);
//	  
//      } else
//      {
//	  return 0;
//      }  
//      
//      return 1;
//}



int DENSOrobot::VelCtrl(const Mat VelScrew)
{
	// Calc Vel Jacob
	Mat VJacob, A_16;
	getVelJacob(VJacob, A_16);
	
	// Calc dTheta
	double det = determinant(VJacob);
	cout << "Singularity check - Det value = " << det << endl << endl;
	Mat theta_d = VJacob.inv(DECOMP_SVD)*VelScrew;   // theta_d 6x1

	// Calc JPose1
	Mat JPose1;
	calcJPose1(theta_d.t(), JPose, JPose1);  // JPose1 1x6
	
	// Calc TrajPlanner
	Mat traj;
	// JPose: llv coord sys
	// JPose1: llv coord sys
	int ret = JTrajPlanner(JPose1);     // RISK!!!

	if (ret)
	{
		//MATmtxdisp(traj/CV_PI*180);
		//DstPose.copyTo(OrgPoseJnt);

		//getchar();
		return 1;
	}
	else 
	{
		//printf("there is no good solution!!\n");
		//getchar();
		return 0;
	}

	return 1;
}


int DENSOrobot::llv2intCoordSys(Mat& Pose, int mode)
{	
	// Shift coordinates
	Mat dfii = Mat::zeros(1, 6, CV_64FC1);
	dfii.at<double>(0) = 0;
	dfii.at<double>(1) = -CV_PI/2;
	dfii.at<double>(2) = -CV_PI/2;
	dfii.at<double>(3) = -CV_PI;
	dfii.at<double>(4) = -CV_PI;
	dfii.at<double>(5) = -CV_PI;

	if (mode)
	{
		Pose = Pose - dfii;   // RISK!!  
	} else
	{
		Pose = Pose + dfii;   // RISK!! to llv
	}

	return 1;
}



// v2.0 (14/10/14) add mode to distinct joint and xyz control modes
int DENSOrobot::calcPose1(const Mat theta_d, const Mat JPose0, Mat& JPose1, int mode)
{
	assert(theta_d.rows == 6 && theta_d.cols == 1);
	assert(JPose0.rows == 6 && JPose0.cols == 1);

	if (mode == CTRL_MODE_JOINT)
	{
		calcJPose1(theta_d, JPose0, JPose1);
	    
	} else 
	  if (mode == CTRL_MODE_XYZ)
	  {
		// Cartesian XYZ mode
		Mat screw = theta_d;
		Mat CPose0 = JPose0;
		
		double rbdelta_t = 0.01;   // make it small to refine discretization. 0.


		Mat CPose1 = CPose0 + screw*rbdelta_t;
		
		// check CPose1 within workspace (Optional)
		
		// return Cartesian Pose
		CPose1.copyTo(JPose1);
	    
	  } else
	  {
		  return 0;
	  }  
	  
	  return 1;   
};



int DENSOrobot::VelCtrl(const Mat VelScrew, int mode)
{
	// Assertion if size of VelScrew is 6x1
	MATmtxdisp(VelScrew);
	assert(VelScrew.rows == 6 && VelScrew.cols == 1);

	Mat traj;

	int ret = 1;
    
    if (mode == CTRL_MODE_JOINT)
    {
		ret = VelCtrl(VelScrew);
	
    } else 
      if (mode == CTRL_MODE_XYZ)
      {
		  Mat CPose0 = CPose;  // assume CPose is always updated properly
		  Mat CPose1;

		  int ret1 = calcPose1(VelScrew, CPose0, CPose1, CTRL_MODE_XYZ);

		  // combine ret from different module calls
		  ret = ret && ret1;

		  // Calc TrajPlanner		
		  // JPose here has a fake value since it is never updated in CTRL_MODE_XYZ
		  int ret2 = TrajPlanner(CPose, CPose1, traj, CTRL_MODE_XYZ);
		  ret = ret && ret2;
	  
      } else
      {
			ret = 0;
      }  
      
      return ret;
}




// v2.0 (14/10/14) update state value directly from caller depending on control mode (joint/xyz)
int DENSOrobot::inputPose(const Mat Pose_inp, int mode)
{
	if (mode == CTRL_MODE_JOINT)
	{
	    updateJPose(Pose_inp);
	    
	} else 
		if (mode == CTRL_MODE_XYZ)
		{
		      Pose_inp.copyTo(CPose);
		  
		} else
		{
		      // error occurs
		      return 0;
		}
		
	return 1;
};



// OrgPose = [j1 j2 j3 j4 j5 j6] (rad) or [x y z r p y] (rad & mm) -> this avoids calculating twice Inv Kine
// DstPose = [j1 j2 j3 j4 j5 j6] (rad) or [x y z r p y] (rad & mm)
// v2.0 (WORKING 14/10/14) add mode to distinct joint and xyz control mode
int DENSOrobot::TrajPlanner(const Mat OrgPose, const Mat DstPose, Mat& BestTraj, int mode)
{

    int ret = 1;
      
    if (mode == CTRL_MODE_JOINT)
    {
	ret = TrajPlanner(OrgPose, DstPose, BestTraj);

    } else 
      if (mode == CTRL_MODE_XYZ)
      {
			// OrgPose & DstPose is [x y z r p y]
			Mat CPose0 = OrgPose;    // here: CPose0 will not be used
			Mat CPose1 = DstPose;
		    
			double* joints = new double[8];  // allocate/initialize a pointer

			Mat SOL = Mat::zeros(6, 1, CV_64FC1);
			CPose1.copyTo(SOL);

			// Return the best choice -> here is directly from CPose1 without any selection
			SOL.copyTo(BestTraj);

			//MATmtxdisp(BestTraj/CV_PI*180);

			// if RS232 is used, command is also transmitted
			if (DENSOrobot::use_RS232)
			{
				// test pose (XYZ mode): [175.0 0.0 536.0 -180.0 90.0 180.0] <=> [0.0 -45.0 135.0 0.0 0.0 0.0] (joint mode)
				// test pose2 (XYZ mode):[120.0, -150.0, -180.0, 0.0, -CV_PI/2, 0.0]
				// for CTRL_MODE_XYZ, format is [x y z Rx Ry Rz] (mm,Deg)
				joints[0] = BestTraj.at<double>(0); //150.0;  //0.0;   // Joint is in Degree
				//Cost = joints[0];
				joints[1] = BestTraj.at<double>(1);  // 100.0; //100.0; //-45.0;
				//Cost = joints[1];
				joints[2] = BestTraj.at<double>(2); //480.0; //130.0;
				//Cost = joints[2];
				joints[3] = BestTraj.at<double>(3)/CV_PI*180;  //2.93; //-180.0; //0.0;
				//Cost = joints[3];
				joints[4] = BestTraj.at<double>(4)/CV_PI*180;  //90; //90.0;  //0.0;
				//Cost = joints[4];
				joints[5] = BestTraj.at<double>(5)/CV_PI*180; // 2.93; //180.0;  //0.0;
				//Cost = joints[5];
				joints[6] = 30.0;  // speed
				joints[7] = 1.0;   // gripper
			    
				// call old code
				//serialPort.Write(joints);

				// call new code
				serialPort.Write(joints, 1);

			}

			//MATmtxdisp(BestTraj);

			BestTraj.copyTo(CPose);  // updated to CPose 14/10/14

			//MATmtxdisp(CPose);
  
      } else
      {
			ret = 0;
      }  
      
      return ret;
  
}


int DENSOrobot::setMode(OPERATE_MODE mode)
{
	
	operMode = mode;

	return 1;
}

int DENSOrobot::getMode(OPERATE_MODE &mode)
{
	int ret = 1;

	mode = operMode;

	return ret;
}
